import serial
import struct
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header

class SerialNode:
    def __init__(self, ser):
        rospy.init_node('master_receiver', anonymous=True)
        self.ser = ser
        self.master_pub = rospy.Publisher('/master/joint_left', JointState, queue_size=10)
        
        self.button = (0, 0, 0)  # 按钮状态
        self.master_pos = (0, 0, 0, 0, 0, 0)  # 主臂当前关节位置

        # 初始化串口
        self.ser = serial.Serial(port="/dev/ttyACM0", baudrate=115200, timeout=1) #串口需改变
        if self.ser.isOpen():
            rospy.loginfo("Serial port opened successfully")
        else:
            rospy.logerr("Failed to open serial port")

    def run(self):
        rate = rospy.Rate(55)  # 设置发布频率为 55 Hz
        while not rospy.is_shutdown():
            # 检查缓冲区是否有数据
            bytes_waiting = self.ser.in_waiting
            rospy.loginfo("Bytes waiting in buffer: %d", bytes_waiting)
            
            if bytes_waiting > 0:
                # 尝试读取字节直到找到有效的头（0xA5）
                while self.ser.in_waiting > 0:
                    head = self.ser.read(1)
                    if head == b'\xA5':
                        # 找到数据头后，读取后续24个字节
                        data = self.ser.read(24)
                        if len(data) == 24:
                            # 解析数据为6个float值
                            self.master_pos = struct.unpack('<6f', data)
                            self.publish_master_action()
                            break  # 成功处理数据后，跳出循环
                        else:
                            rospy.logwarn("Incomplete data received, waiting for next header.")
                            break  # 如果数据不完整，则跳出循环，重新查找数据头
                    else:
                        # 丢弃无效的字节，继续读取直到找到0xA5
                        rospy.logwarn("Invalid data header found, discarding byte.")
                        self.ser.reset_input_buffer()
            
            rate.sleep()

    def publish_master_action(self):
        msg = JointState()
        msg.header = Header()
        msg.header.stamp = rospy.Time.now()
        msg.name = ["master_joint1", "master_joint2", "master_joint3",
                    "master_joint4", "master_joint5", "master_joint6"]
        msg.position = list(self.master_pos)
        msg.velocity = []
        msg.effort = []
        self.master_pub.publish(msg)
        rospy.loginfo("Published master arm position: %s", msg.position)

def main():
    ser = serial.Serial(port="/dev/ttyACM0", baudrate=115200, timeout=1)
    serialNode = SerialNode(ser)

    try:
        serialNode.run()  # 运行节点
    except rospy.ROSInterruptException:
        rospy.logerr("ROS node interrupted")
    except KeyboardInterrupt:
        rospy.loginfo("KeyboardInterrupt")
    finally:
        ser.close()  # 关闭串口
        rospy.loginfo("Serial port closed")

if __name__ == "__main__":
    main()